#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    Gyro,           sensorGyro)
#pragma config(Sensor, in2,    ArmPot,         sensorPotentiometer)
#pragma config(Sensor, in3,    flipperpot,     sensorPotentiometer)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_3,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_4,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_5,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_6,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           LeftBaseBack,  tmotorVex393HighSpeed, openLoop)
#pragma config(Motor,  port2,           LeftBaseFront, tmotorVex393HighSpeed, openLoop)
#pragma config(Motor,  port3,           RightArm,      tmotorVex393, PIDControl, encoder, encoderPort, I2C_5, 1000)
#pragma config(Motor,  port4,           LeftArm,       tmotorVex393, PIDControl, reversed, encoder, encoderPort, I2C_6, 1000)
#pragma config(Motor,  port5,           ConveyorMotor, tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port6,           FlipperMotorLeft, tmotorVex269, openLoop)
#pragma config(Motor,  port7,           FlipperMotorRight, tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port8,           RightBaseBack, tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port9,           RightBaseFront, tmotorVex393HighSpeed, openLoop, reversed)
#pragma config(Motor,  port10,          turntablemotor, tmotorVex393HighSpeed, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// Pot Values
int Trough = 32; // placeholder value for the moment
int HighGoal = 100; // "
int HighLimit = 120; // "
int LowLimit = 0; // "
int flipperfloor = 0; // "
int flipperdump = -50; // "
int onesquare = 50; // "
int twosquares = 100; // "
int threesquares = 150; // "
int degrees10 = 900;

// Arm Commands
bool goup = true;
bool godown = true;

///////////////////////////////////////Start Autonomous Voids//////////////////////////////////////////////////////////

void score()
{
		//part 1 go forward one square
	while(nMotorEncoder(LeftBaseFront) < onesquare)
  {
		if(nMotorEncoder(RightBaseFront) == nMotorEncoder(LeftBaseFront)) // If rightEncoder has counted the same amount as leftEncoder:
		{
			// Move Forward
			motor[RightBaseFront] = 127;		    // Right Motor is run at power level 127
			motor[RightBaseBack] = 127;
			motor[LeftBaseFront]  = 127;		    // Left Motor is run at power level 107
			motor[LeftBaseFront]  = 127;
		}
		else if(nMotorEncoder(RightBaseFront) > nMotorEncoder(LeftBaseFront))	// If rightEncoder has counted more encoder counts
		{
			// Turn slightly right
			motor[RightBaseFront] = 107;		    // Right Motor is run at power level 107
			motor[RightBaseBack] = 107;
			motor[LeftBaseFront]  = 127;		    // Left Motor is run at power level 127
			motor[LeftBaseFront]  = 127;
		}
		else	// Only runs if leftEncoder has counted more encoder counts
		{
			// Turn slightly left
			motor[RightBaseFront] = 127;		    // Right Motor is run at power level 127
			motor[RightBaseBack] = 127;
			motor[LeftBaseFront]  = 107;		    // Left Motor is run at power level 107
			motor[LeftBaseFront]  = 107;
		}
		motor[RightBaseFront] = 0;		    // Right Motor is run at power level 0
		motor[RightBaseBack] = 0;
		motor[LeftBaseFront]  = 0;		    // Left Motor is run at power level 0
		motor[LeftBaseFront]  = 0;
	}
  //part 2 turn 90 degrees
  //While the absolute value of the gyro is less than the desired rotation...
  while(abs(SensorValue[Gyro]) < degrees10)
  {
    //...continue turning
    motor[RightBaseFront] = 25;
    motor[RightBaseBack] = 25;
    motor[LeftBaseFront] = -25;
    motor[LeftBaseFront]  = -25;
  }

  //Brief brake to stop some drift
  motor[RightBaseFront] = -5;
  motor[RightBaseBack] = -5;
  motor[LeftBaseFront] = 5;
  motor[LeftBaseFront]  = 5;
  wait1Msec(250);
  //part 3 raise arms to trough
  while(SensorValue(ArmPot) < Trough)
  {
  	motor(RightArm) = 63;
  	motor(LeftArm) = 63;
  }
  motor(RightArm) = 5;
  motor(LeftArm) = 5;
  // part 4 go forward 3 squares
	while(nMotorEncoder(LeftBaseFront) < threesquares)
  {
		if(nMotorEncoder(RightBaseFront) == nMotorEncoder(LeftBaseFront)) // If rightEncoder has counted the same amount as leftEncoder:
		{
			// Move Forward
			motor[RightBaseFront] = 127;		    // Right Motor is run at power level 127
			motor[RightBaseBack] = 127;
			motor[LeftBaseFront]  = 127;		    // Left Motor is run at power level 107
			motor[LeftBaseFront]  = 127;
		}
		else if(nMotorEncoder(RightBaseFront) > nMotorEncoder(LeftBaseFront))	// If rightEncoder has counted more encoder counts
		{
			// Turn slightly right
			motor[RightBaseFront] = 107;		    // Right Motor is run at power level 107
			motor[RightBaseBack] = 107;
			motor[LeftBaseFront]  = 127;		    // Left Motor is run at power level 127
			motor[LeftBaseFront]  = 127;
		}
		else	// Only runs if leftEncoder has counted more encoder counts
		{
			// Turn slightly left
			motor[RightBaseFront] = 127;		    // Right Motor is run at power level 127
			motor[RightBaseBack] = 127;
			motor[LeftBaseFront]  = 107;		    // Left Motor is run at power level 107
			motor[LeftBaseFront]  = 107;
		}
		motor[RightBaseFront] = 0;		    // Right Motor is run at power level 0
		motor[RightBaseBack] = 0;
		motor[LeftBaseFront]  = 0;		    // Left Motor is run at power level 0
		motor[LeftBaseFront]  = 0;
	}
	//part 4 flipper dump
	if(SensorValue(flipperpot) > flipperdump)
	{
    motor(FlipperMotorLeft) = -63;
    motor(FlipperMotorRight) = -63;
	}
	else
	{
	  motor(FlipperMotorLeft) = 0;
    motor(FlipperMotorRight) = 0;
	}
}

void pickupbonus()
{
	//part 1 go forward one square
	while(nMotorEncoder(LeftBaseFront) < onesquare)
  {
		if(nMotorEncoder(RightBaseFront) == nMotorEncoder(LeftBaseFront)) // If rightEncoder has counted the same amount as leftEncoder:
		{
			// Move Forward
			motor[RightBaseFront] = 127;		    // Right Motor is run at power level 127
			motor[RightBaseBack] = 127;
			motor[LeftBaseFront]  = 127;		    // Left Motor is run at power level 107
			motor[LeftBaseFront]  = 127;
		}
		else if(nMotorEncoder(RightBaseFront) > nMotorEncoder(LeftBaseFront))	// If rightEncoder has counted more encoder counts
		{
			// Turn slightly right
			motor[RightBaseFront] = 107;		    // Right Motor is run at power level 107
			motor[RightBaseBack] = 107;
			motor[LeftBaseFront]  = 127;		    // Left Motor is run at power level 127
			motor[LeftBaseFront]  = 127;
		}
		else	// Only runs if leftEncoder has counted more encoder counts
		{
			// Turn slightly left
			motor[RightBaseFront] = 127;		    // Right Motor is run at power level 127
			motor[RightBaseBack] = 127;
			motor[LeftBaseFront]  = 107;		    // Left Motor is run at power level 107
			motor[LeftBaseFront]  = 107;
		}
		motor[RightBaseFront] = 0;		    // Right Motor is run at power level 0
		motor[RightBaseBack] = 0;
		motor[LeftBaseFront]  = 0;		    // Left Motor is run at power level 0
		motor[LeftBaseFront]  = 0;
	}
	//part 2 turn 90 degrees
  //While the absolute value of the gyro is less than the desired rotation...
  while(abs(SensorValue[Gyro]) < degrees10)
  {
    //...continue turning
    motor[RightBaseFront] = 25;
    motor[RightBaseBack] = 25;
    motor[LeftBaseFront] = -25;
    motor[LeftBaseFront]  = -25;
  }

  //Brief brake to stop some drift
  motor[RightBaseFront] = -5;
  motor[RightBaseBack] = -5;
  motor[LeftBaseFront] = 5;
  motor[LeftBaseFront]  = 5;
  wait1Msec(250);
  //part 3 go forward 2 squares
	while(nMotorEncoder(LeftBaseFront) < twosquares)
  {
		if(nMotorEncoder(RightBaseFront) == nMotorEncoder(LeftBaseFront)) // If rightEncoder has counted the same amount as leftEncoder:
		{
			// Move Forward
			motor[RightBaseFront] = 127;		    // Right Motor is run at power level 127
			motor[RightBaseBack] = 127;
			motor[LeftBaseFront]  = 127;		    // Left Motor is run at power level 107
			motor[LeftBaseFront]  = 127;
		}
		else if(nMotorEncoder(RightBaseFront) > nMotorEncoder(LeftBaseFront))	// If rightEncoder has counted more encoder counts
		{
			// Turn slightly right
			motor[RightBaseFront] = 107;		    // Right Motor is run at power level 107
			motor[RightBaseBack] = 107;
			motor[LeftBaseFront]  = 127;		    // Left Motor is run at power level 127
			motor[LeftBaseFront]  = 127;
		}
		else	// Only runs if leftEncoder has counted more encoder counts
		{
			// Turn slightly left
			motor[RightBaseFront] = 127;		    // Right Motor is run at power level 127
			motor[RightBaseBack] = 127;
			motor[LeftBaseFront]  = 107;		    // Left Motor is run at power level 107
			motor[LeftBaseFront]  = 107;
		}
		motor[RightBaseFront] = 0;		    // Right Motor is run at power level 0
		motor[RightBaseBack] = 0;
		motor[LeftBaseFront]  = 0;		    // Left Motor is run at power level 0
		motor[LeftBaseFront]  = 0;
	}
}

void flipperdown()
{
	if(SensorValue(flipperpot) > flipperfloor)
	{
    motor(FlipperMotorLeft) = -63;
    motor(FlipperMotorRight) = -63;
	}
	else
	{
	  motor(FlipperMotorLeft) = 0;
    motor(FlipperMotorRight) = 0;
	}
}

///////////////////////////////////////Start User Control Voids/////////////////////////////////////////////////////////
// The controls for the user control base.  It has PID in it (nMotorPIDSpeedCtrl[motorX] = mtrSpeedReg in #pragma config)
// as now doesn't have a ramp motor.
void base()
{
  motor(LeftBaseBack) = vexRT(Ch3);
  motor(LeftBaseFront) = vexRT(Ch3);
  motor(RightBaseBack) = vexRT(Ch2);
  motor(RightBaseFront) = vexRT(Ch2);
}

void Arms()
{
	if(vexRT(Ch5Xmtr2) == 127)
  {
    goup = true;
    while(goup == true)
    {
      motor(RightArm) = 127;
      motor(LeftArm) = 127;
      if(SensorValue(ArmPot) == Trough || SensorValue(ArmPot) ==  HighGoal || SensorValue(ArmPot) >= HighLimit)
      {
        goup = false;
        motor(RightArm) = 5;
        motor(LeftArm) = 5;
      }
    }
  }

  motor[RightArm] = vexRT(Ch2Xmtr2)/2; // Lifter Motor 1
  motor[LeftArm] = vexRT(Ch2Xmtr2)/2; // Lifter Motor 2

	if(vexRT(Ch5Xmtr2) == -127)
  {
    godown = true;
    while(godown == true)
    {
      motor(RightArm) = -127;
      motor(LeftArm) = -127;
      if(SensorValue(ArmPot) == Trough || SensorValue(ArmPot) ==  HighGoal || SensorValue(ArmPot) <= LowLimit)
      {
        godown = false;
        motor(RightArm) = 5;
        motor(LeftArm) = 5;
      }
    }
  }
}

void Conveyor()
{
	if(vexRT[Btn7UXmtr2] == 1)
	{
		motor[ConveyorMotor] = 127;
	}
  if(vexRT[Btn7DXmtr2] == 1)
	{
		motor[ConveyorMotor] = -127;
	}
	if(vexRT[Btn7UXmtr2] == 0 && vexRT[Btn7DXmtr2] == 0)
	{
		motor[ConveyorMotor] = 0;
	}
}

void Flipper()
{
	if(vexRT[Btn6UXmtr2] == 1)
	{
		motor[FlipperMotorLeft] = 63;
		motor[FlipperMotorRight] = 63;
	}
	if(vexRT[Btn6DXmtr2] == 1)
	{
		motor[FlipperMotorLeft] = -63;
		motor[FlipperMotorRight] = -63;
	}
	if(vexRT[Btn6DXmtr2] == 0 && vexRT[Btn6UXmtr2] == 0)
	{
		motor[FlipperMotorLeft] = 5;
		motor[FlipperMotorRight] = 5;
	}
}

void turntable()
{
  if(vexRT[Btn5UXmtr2] == 1)
  {
  	motor[turntablemotor] = 127;
  }
  if(vexRT[Btn5DXmtr2] == 1)
  {
  	motor[turntablemotor] = -127;
 	}
 	if(vexRT[Btn5UXmtr2] == 0 && vexRT[Btn5DXmtr2] == 0)
 	{
 		motor[turntablemotor] = 0;
 	}
}

/*task antistallleft()
{
  int quadPrev = 0; //stores previous encoder value
  float threshold = .01; //variable to increase or decrease the torque at which the motor stops.
  nMotorEncoder(LeftBaseFront) = 0; //reset encoder
  wait1Msec(100); //initialize
  while(true)
    {
    if(motor[LeftBaseFront] > 20 || motor[LeftBaseFront] < -20) //checks if motor is running at a reasonable speed
      {
      wait1Msec(100); //checks for stop and go movement
      while(motor[LeftBaseFront] > 20 || motor[LeftBaseFront] < -20)
        {
        quadPrev = nMotorEncoder(LeftBaseFront); //set quadPrev equal to the current encoder value
        wait1Msec(100);//let the motor run a little
        if(abs(quadPrev - nMotorEncoder(LeftBaseFront)) < (abs(motor[LeftBaseFront]) * threshold))
          {
        //IMPORTANT PART: checks if the axle has moved the correct amount in proportion to the motor speed
          motor[LeftBaseFront] = 0; // stops motor
          ClearTimer(T1);
          hogCPU(); //stops task main from executing until the release button is pressed
          while(time1[T1] < 1000)
            { //this button releases the motor
            motor[LeftBaseFront] = 0; //stops the motor which is not running correctly
            motor[LeftBaseBack] = 0;

            //insert all user controls except for the motor that is being stopped
  					wait1Msec(100);
    				// run the arms program
    				Arms();
    				// run the conveyor program
    				Conveyor();
   					// run the flipper program
				    Flipper();
    				// run the turntable program
    				turntable();
          }
          releaseCPU(); //starts running task main again
        }
      }
    }
  }
}

task antistallright()
{
  int quadPrev = 0; //stores previous encoder value
  float threshold = .01; //variable to increase or decrease the torque at which the motor stops.
  nMotorEncoder(RightBaseFront) = 0; //reset encoder
  wait1Msec(100); //initialize
  while(true)
    {
    if(motor[RightBaseFront] > 20 || motor[RightBaseFront] < -20) //checks if motor is running at a reasonable speed
      {
      wait1Msec(100); //checks for stop and go movement
      while(motor[RightBaseFront] > 20 || motor[RightBaseFront] < -20)
        {
        quadPrev = nMotorEncoder(RightBaseFront); //set quadPrev equal to the current encoder value
        wait1Msec(100);//let the motor run a little
        if(abs(quadPrev - nMotorEncoder(RightBaseFront)) < (abs(motor[RightBaseFront]) * threshold))
          {
        //IMPORTANT PART: checks if the axle has moved the correct amount in proportion to the motor speed
          motor[RightBaseFront] = 0; // stops motor
          ClearTimer(T1);
          hogCPU(); //stops task main from executing until the release button is pressed
          while(time1[T1] < 1000)
            { //this button releases the motor
            motor[RightBaseFront] = 0; //stops the motor which is not running correctly
            motor[RightBaseBack] = 0;

            //insert all user controls except for the motor that is being stopped
    				// run the arms program
    				Arms();
    				// run the conveyor program
    				Conveyor();
   					// run the flipper program
				    Flipper();
    				// run the turntable program
    				turntable();
          }
          releaseCPU(); //starts running task main again
        }
      }
    }
  }
}
*/
task main()
{

/*
task autonomous()
{
	if(nLCDButtons == leftButton)
	{
		in this program we will score in the trough
		score();
  }
  if(nLCDButtons == RightButton)
  {
  	in this program we will pick up the bonus sack
  	pickupbonus();
	}
	if(nLCDButtons == MiddleButton)
	{
		in this program it will put the flipper down
		flipperdown();
	}
}
*/
	bLCDBacklight = true; // have LCD backlight on
	string mainBattery, backupBattery;
  //Clear the encoders associated with the left and right motors
  nMotorEncoder[RightBaseBack] = 0;
  nMotorEncoder[LeftBaseBack] = 0;
  nMotorEncoder[RightBaseFront] = 0;
  nMotorEncoder[LeftBaseFront] = 0;

	// Run Forever
  while(1)
  {
  	// Displays the battery level on the LCD
  	clearLCDLine(0);                                            // Clear line 1 (0) of the LCD
  	clearLCDLine(1);                                            // Clear line 2 (1) of the LCD

  	//Display the Primary Robot battery voltage
  	displayLCDString(0, 0, "Primary: ");
  	sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed
  	displayNextLCDString(mainBattery);

  	//Display the Backup battery voltage
  	displayLCDString(1, 0, "Backup: ");
  	sprintf(backupBattery, "%1.2f%c", BackupBatteryLevel/1000.0, 'V');    //Build the value to be displayed
  	displayNextLCDString(backupBattery);

  	//Short delay for the LCD refresh rate
  	wait1Msec(100);
		// Run the base program
    base();
    // run the arms program
    Arms();
    // run the conveyor program
    Conveyor();
    // run the flipper program
    Flipper();
    // run the turntable program
    turntable();
  }
}
